/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.io.ObjectInputStream.GetField;
import java.util.LinkedList;

import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.actions.Action;
import robocup.component.actions.ChangeViewModeAction;
import robocup.component.actions.Action.TYPE;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.AuralInfo;
import robocup.component.infotypes.ChangeOpponentTypeInfo;
import robocup.component.infotypes.ChangePlayerTypeInfo;
import robocup.component.infotypes.CoachSayInfo;
import robocup.component.infotypes.DynamicObjectInfo;
import robocup.component.infotypes.Info;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.ServerParamInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.speechacts.MessageFactory;
import robocup.component.speechacts.MessageFactory.MESSAGES;
import robocup.component.speechacts.coachlanguage.TypeConstants;
import robocup.component.tactics.AbstractState.STATES;
import robocup.component.worldmodel.BallModel;
import robocup.component.worldmodel.CLangModel;
import robocup.component.worldmodel.CollisionModel;
import robocup.component.worldmodel.IModel;
import robocup.component.worldmodel.LongTermMemory;
import robocup.component.worldmodel.MeModel;
import robocup.component.worldmodel.NeckRotator;
import robocup.component.worldmodel.PlayModeModel;
import robocup.component.worldmodel.PlayersModel;
import robocup.component.worldmodel.ShortTermMemory;
import robocup.component.worldmodel.PlayersModel.DOMINATED_SIDE;
import robocup.component.worldmodel.ShortTermMemory.VEKTORS;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.DynamicObject;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.StaticObject;
import robocup.component.worldobjects.Player.ROLE;
import robocup.connection.TeamSoccerScopeMediator;

/**
 * 
 * This class contains all knowledge about the current world state. It is
 * updated each time an info arrives from the SServer. 
 * 
 * The world model is divided into different submodels:
 * 
 * MeModel        - a model containing all knowledge about the player 
 *                  (position, speed, etc.)
 * BallModel      - a model containing all knowledge about the ball
 * PlayersModel   - a model containing all knowledge about other Players
 * CollisionModel - a model that determines wether we collided or not
 *                  (if we collided some of the informations have to be
 *                  interpreted a different way)
 * PlayModeModel  - a model containing all knowledge about the last play
 *                  modes and referee messages
 * NeckRotator    - a model containing all knowledge about what directions were
 *                  seen
 * CLangModel     - a model containing all knowledge about infos sent from the 
 *                  coach
 * 
 * @author rw, smarqoo, wild, till, haeder
 */
public class WorldModel {

	private FileWriter subor;
	
    // Parent RobocupAgent
    private RobocupAgent myMaster;

    //all possible player configs
    private PConf configs[] = null;

    //the models
    private MeModel me;
    private BallModel ball;
    private PlayersModel players;
    private CollisionModel collisionModel;
    private PlayModeModel playModeModel;
    private NeckRotator neckRotator = null;
    private CLangModel clangModel = null;
    //an array containing all models
    private IModel[] models;
    //a memory used as cache (reseted after each cycle)
    private ShortTermMemory shortTermMemory = new ShortTermMemory();

    //a memeory storing long term data
    private LongTermMemory longTermMemory;

    //a reference to the player
    private Player self;

    //the message factory used for decoding arriving say messages
    private MessageFactory mf;

    //number of sense body infos
    private int bodyCycleCounter = 0;

    //world is based on a VI (not on a BS)
    private boolean viBased = false;

    //goals scored we
    private int goalsTeam = 0;

    //goals scored other
    private int goalsOther = 0;

    //the last cycle we moved
    private int lastMoveCycle = -1;

    //a counter for all move actions
    //used for telling if a move action was successful
    private int moveCount = 0;

    //the opponents teamname
    private String opponentTeamName = "";

    //a dummy vektor
    private Vektor dummyVektor = new Vektor();

    /**
     * Default constructor.
     *
     * Initializes all the submodels.
     *
     * @param agent -
     *            the agent object, to whom the world-model belongs (parent)
     */
    public WorldModel(RobocupAgent agent) {

        this.myMaster = agent;
        this.longTermMemory = new LongTermMemory(this);

        this.neckRotator = new NeckRotator(this);

        // initially set all values
        this.self = new Player(-1, -1, (this.myMaster.isGoalie()) ? 1 : 2,
                new Vektor(-1, 0), new Vektor(), 0, 0, 4000.0, 1.0, 1.0, true,
                true, this.myMaster.isGoalie(), new PConf());
        this.self.setReliable(true);

        this.playModeModel = new PlayModeModel(this);
        this.me = new MeModel(this);
        this.ball = new BallModel(this);
        this.players = new PlayersModel(this);
        this.collisionModel = new CollisionModel(this);
        this.clangModel = new CLangModel(this);

        this.models = new IModel[]{this.playModeModel, this.me, this.ball,
                    this.players, this.collisionModel, this.neckRotator,
                    this.clangModel};

        this.mf = new MessageFactory();
    } // end of constructor WorldModel(FactBase iFactBase)

    /**
     * inits the self after the arrial of an init info
     * and sends a reference of it to some sub models.
     *
     */
    private void initSelf() {

        this.self = new Player(-1, -1, this.myMaster.getNumber(), new Vektor(
                -1, 0), new Vektor(), 0, 0, 4000.0, 1.0, 1.0, true, true,
                this.myMaster.isGoalie(), this.self.getPConf());
        this.me.initSelf(this.self);
        this.players.initSelf(this.self);
    }

    /**
     * updates the world.
     *
     * called from the RobocupAgent each time an info arrives.
     *
     * @param info -
     *            new info
     */
    public synchronized void update(Info info) {

        if (info == null) {
            System.out.println("WM.update(): called without info.");
            return;
        }

        // reset the shortTermMemory
        this.shortTermMemory.resetAll(info.getInfoType());

        // insert info in longTermMemory
        this.longTermMemory.insertInfo(info);

        //depending on the info type there is a different
        //update logic
        switch (info.getInfoType()) {

            // INIT
            case Info.TYPE_INIT:
                initSelf();
                break;

            // SENSE
            case (Info.TYPE_SENSE):
                if (this.getNumber() < 0) {
                    return;
                } else {
                    this.bodyCycleCounter++;
                }

                SenseBodyInfo sbInfo = (SenseBodyInfo) info;

                //did our moe action succeed?
                if (this.moveCount != sbInfo.getCount(TYPE.MOVE)) {
                    this.lastMoveCycle = info.getCycle();
                    this.moveCount = sbInfo.getCount(TYPE.MOVE);
                }

                this.viBased = false;

                //forward the info to all submodels
                for (IModel model : this.models) {
                    model.update(sbInfo);
                }
                break;

            // VISUAL
            case Info.TYPE_VISUAL:

                //only update after we got the init info containing our number
                if (this.getNumber() < 0) {
                    return;
                }

                this.viBased = true;

                VisualInfo vInfo = (VisualInfo) info;

                //forward the info to all submodels
                for (IModel model : this.models) {
                    model.update(vInfo);
                }
                break;

            // AURAL
            case Info.TYPE_AURAL:

                //only update after we got the init info containing our number
                if (this.getNumber() < 0) {
                    return;
                }

                AuralInfo aInfo = (AuralInfo) info;

                if (aInfo instanceof PlayModeInfo) {

                    PlayModeInfo pmInfo = (PlayModeInfo) aInfo;

                    // if the playMode changes to goal_? -> add the goal
                    if (pmInfo.getPlayModeType() == PLAY_MODE.GOAL_OTHER && this.playModeModel.getLastPlayModeMessageType() != PLAY_MODE.GOAL_OTHER) {
                        this.goalsOther++;
                    } else if (pmInfo.getPlayModeType() == PLAY_MODE.GOAL_OWN && this.playModeModel.getLastPlayModeMessageType() != PLAY_MODE.GOAL_OWN) {
                        this.goalsTeam++;
                    }

                    //forward the info to all submodels
                    for (IModel model : this.models) {
                        model.update(pmInfo);
                    }
                } else if (aInfo instanceof RefereeInfo) {
                    RefereeInfo rInfo = ((RefereeInfo) aInfo);

                    //forward the info to all submodels
                    for (IModel model : this.models) {
                        model.update(rInfo);
                    }
                }

                break;

            // AURAL - Player Say Info
            case Info.TYPE_AURAL_PSI:

                //decode psi once and forward it to submodels
                PlayerSayInfo psiInfo = ((PlayerSayInfo) info);
                //debug> System.out.println(psiInfo.getMessage() + " - je moja obdrzana sprava pri update worldmodelu na zaklade PSI");
                this.mf.generateMessages(psiInfo.getMessage(), getBodyCycle());
                for (IModel model : this.models) {
                    model.update(psiInfo);
                }

                //forward the psi to the gui (if running)
                if (myMaster.useGui) {
                    int pIndex = this.getNumber() - 1;
                    if (myMaster.isTeamEast) {
                        pIndex += 11;
                    }
                    StringBuffer strBuff = new StringBuffer();
                    Player p = getPlayerByNumber(psiInfo.getNumber(), true);
                    ROLE role = null;
                    if (p != null) {
                        role = p.getDefaultRole();
                    }
                    strBuff.append(role).append(": ");
                    for (MESSAGES m : mf.getMessageTypes()) {
                        strBuff.append(m);

                        // only use this if you don't care about performance
                        if (m == MESSAGES.COMPLETE_PLAYER) {
                            mf.decodeMessageNumber(0);
                            strBuff.append(mf.getNumber());
                        }
                        strBuff.append(",");
                    }
                    TeamSoccerScopeMediator.soccerScopeAddHearMessageToScene(
                            pIndex, new StringBuffer().append(psiInfo.getNumber()).append("-").append(strBuff).toString());
                }
                break;

            // AURAL - Coach Say Info
            case Info.TYPE_AURAL_CSI:
                CoachSayInfo csi = (CoachSayInfo) info;

                TypeConstants.MESSAGETYPE type = csi.getMessageType();

                switch (type) {

                    case FREEFORM:
                        this.mf.generateMessages(csi.getFreeformMessage(),
                                getBodyCycle());
                        // update the mental-model as well
                        this.players.update(csi);
                        this.ball.update(csi);

                        //forward the csi to the gui (if running)
                        if (this.myMaster.useGui) {
                            StringBuffer strBuff = new StringBuffer();
                            strBuff.append("TRAINER: ");
                            for (MESSAGES m : this.mf.getMessageTypes()) {
                                strBuff.append(m + " ");
                            }
                            int pIndex = this.getNumber() - 1;
                            if (this.myMaster.isTeamEast) {
                                pIndex += 11;
                            }
                            TeamSoccerScopeMediator.soccerScopeAddHearMessageToScene(
                                    pIndex, new StringBuffer().append("0-").append(
                                    strBuff).toString());
                        }

                        break;

                    default:
                        this.clangModel.update(csi);
                        break;
                }

                break;

            // not used (now part of Info.TYPE_PLAYERTYPE)
            case Info.TYPE_PPARAM:
                break;

            // set ServerParam
            case Info.TYPE_SPARAM:
                SConf.setInstance(((ServerParamInfo) info).getSConf());
                break;

            // PlayerType: get all avaiable PlayerTypes (7 by default)
            case Info.TYPE_PTYPE:
                if (this.configs == null) {
                    PlayerTypesInfo pti = (PlayerTypesInfo) info;
                    if (pti.playerTypesFull()) {
                        this.configs = pti.getPlayerTypes();
                    }

                }
                break;

            // ChangePlayerType: sets teammates and the own PConf by PlayerType
            case Info.TYPE_PLAYERTYPE:
                Player[] team = this.getTeammates();
                ChangePlayerTypeInfo cpti = (ChangePlayerTypeInfo) info;
                if (this.configs != null) {
                    team[cpti.getPlayerNumber() - 1].setPConf(this.configs[cpti.getPlayerType()]);
                    team[cpti.getPlayerNumber() - 1].setStatusChanged(true);
                    team[cpti.getPlayerNumber() - 1].setRecovery(SConf.getInstance().RECOVER_MAX);
                    team[cpti.getPlayerNumber() - 1].setEffort(this.configs[cpti.getPlayerType()].EFFORT_MAX);
                    team[cpti.getPlayerNumber() - 1].setStamina(SConf.getInstance().STAMINA_MAX);
                }
                break;

            // ChangeOpponentTypeInfo: In this case, the opponent team changed
            // Player
            case Info.TYPE_OPLAYERTYPE:
                Player[] opponents = this.getOpponents();
                ChangeOpponentTypeInfo coti = (ChangeOpponentTypeInfo) info;
                for (int i = 0; i < opponents.length; i++) {
                    if (opponents[i].getNumber() == coti.getPlayerNumber()) {
                        opponents[i].setPConf(this.configs[0]);
                        opponents[i].setStatusChanged(true);
                    }
                }

                break;
        }
        
        if (subor == null && this.getNumber() > 0) {
        	/*File file = new File("/home/bobor/tmp/svet_hraca_" + this.getNumber() + ".txt");
        	try {
        		subor = new FileWriter(file);
        	} catch (IOException e) {
        		System.out.println("File can not be open!. Error: " + e);
        	}*/
        }
        if (subor != null) {
        	//pomocný výpis modelu sveta
        	try {
        		subor.write("takt " + getCycle() + ": ");
        		subor.write("lopta [" + getBallRef().getAge(getBodyCycle()) + "] ");
        		subor.write(getBallRef().getPosition().getX() + "x" + getBallRef().getPosition().getY() + "; ");
        		Player[] nasi = this.getTeammates();
        		for (int i = 0; i < nasi.length; i++) {
        			subor.write("náš " + nasi[i].getNumber() + " [" + nasi[i].getAge(getBodyCycle()) + "] ");
            		subor.write(nasi[i].getPosition().getX() + "x" + nasi[i].getPosition().getY() + "; ");
        		}
        		Player[] ich = this.getOpponents();
        		for (int i = 0; i < ich.length; i++) {
        			subor.write("ich " + ich[i].getNumber() + " [" + ich[i].getAge(getBodyCycle()) + "] ");
            		subor.write(ich[i].getPosition().getX() + "x" + ich[i].getPosition().getY() + "; ");
        		}
        		subor.write("\n");
        	} catch (IOException e) {
        		System.out.println("File can not be written!. Error: " + e);
        	}
        }
    } // end of method update

    /**
     * The best angle to turn the neck.
     *
     * @param firstAction -
     *            the choosen main action
     * @param viewModeAction -
     *            the choosen view mode action
     * @param a -
     *            calling action (state)
     *
     * @return angle to turn neck (rel to headAngle)
     *
     * TODO
     *
     * - move to Actionfactory
     */
    public double getBestTurnNeckAngle(Action firstAction,
            ChangeViewModeAction viewModeAction, STATES a) {

        double viewWidth = (viewModeAction == null) ? this.getViewWidth()
                : viewModeAction.getViewWidth();
        Player p = this.getSelfRef().cloned();
        p.predictThisAfterAction(firstAction);
        // best view angle
        double angleAbs = this.neckRotator.bestViewDirAbs(p.getBodyDir(),
                viewWidth, a);

        // some corrections....
        double angleToBody = Vektor.normalize(angleAbs - p.getBodyDir());

        // this should never happen
        if (angleToBody < SConf.getInstance().MINNECKANG - this.neckRotator.getSliceWidth() || angleToBody > SConf.getInstance().MAXNECKANG + this.neckRotator.getSliceWidth()) {
            System.out.println("NeckRotator breaks neck: C" + getCycle() + " P" + getNumber() + "State:" + a + " result. Neckangle=" + angleToBody + " abs.Angle=" + angleAbs + " BD:" + p.getBodyDir() + " ND:" + p.getNeckAngle());
            this.neckRotator.showValues(a, 0);
        }

        // as the NeckRotator is discret, it might sometimes return to high
        // values
        // max error is neckRotator.getSliceWidth() (11.5 degree by default)
        if (angleToBody < SConf.getInstance().MINNECKANG) // + 0.001 for avoiding 180� turn in wrong direction
        {
            angleToBody = SConf.getInstance().MINNECKANG + 0.001;
        }
        if (angleToBody > SConf.getInstance().MAXNECKANG) // - 0.001 for avoiding 180� turn in wrong direction
        {
            angleToBody = SConf.getInstance().MAXNECKANG - 0.001;
        }

        return Vektor.normalize(angleToBody - p.getNeckAngle());
    }

    /**
     * The best angle to turn the neck.
     *
     * @param firstAction -
     *            the choosen main action
     * @param viewModeAction -
     *            the choosen view mode action
     * @param a -
     *            calling action (state)
     * @param obj -
     *            preferred object to see
     *
     * @return angle to turn neck (rel to headAngle)
     *
     * TODO
     *
     * - move to Actionfactory
     */
    public double getBestTurnNeckAngle(Action firstAction,
            ChangeViewModeAction viewModeAction, STATES a, DynamicObject obj) {

        double viewWidth = (viewModeAction == null) ? this.getViewWidth()
                : viewModeAction.getViewWidth();
        Player p = this.getSelfRef().cloned();
        p.predictThisAfterAction(firstAction);
        // best view angle
        double angleAbs = neckRotator.bestViewDirAbs(p.getBodyDir(), viewWidth,
                a, obj);

        // some corrections....
        double angleToBody = Vektor.normalize(angleAbs - p.getBodyDir());

        // this should never happen
        if (angleToBody < SConf.getInstance().MINNECKANG - neckRotator.getSliceWidth() || angleToBody > SConf.getInstance().MAXNECKANG + neckRotator.getSliceWidth()) {
            System.out.println("NeckRotator breaks neck: C" + getCycle() + " P" + getNumber() + "State:" + a + " result. Neckangle=" + angleToBody + " abs.Angle=" + angleAbs + " BD:" + p.getBodyDir() + " ND:" + p.getNeckAngle());
            neckRotator.showValues(a, 0);
        }

        // as the NeckRotator is discret, it might sometimes return to high
        // values
        // max error is neckRotator.getSliceWidth() (11.5� by default)
        if (angleToBody < SConf.getInstance().MINNECKANG) // + 0.001 for avoiding 180� turn in wrong direction
        {
            angleToBody = SConf.getInstance().MINNECKANG + 0.001;
        }
        if (angleToBody > SConf.getInstance().MAXNECKANG) // - 0.001 for avoiding 180� turn in wrong direction
        {
            angleToBody = SConf.getInstance().MAXNECKANG - 0.001;
        }

        return Vektor.normalize(angleToBody - p.getNeckAngle());
    }

    /**
     * The best angle to turn the neck.
     *
     * @param firstAction -
     *            the choosen main action
     * @param viewModeAction -
     *            the choosen view mode action
     * @param a -
     *            calling action (state)
     * @param preferred -
     *            preferred (and unwanted) objects to see
     *
     * @return angle to turn neck (rel to headAngle)
     *
     * TODO
     *
     * - move to Actionfactory
     */
    public double getBestTurnNeckAngle(Action firstAction,
            ChangeViewModeAction viewModeAction, STATES a, double[] preferred) {

        double viewWidth = (viewModeAction == null) ? this.getViewWidth()
                : viewModeAction.getViewWidth();
        Player p = this.getSelfRef().cloned();
        p.predictThisAfterAction(firstAction);
        // best view angle
        double angleAbs = neckRotator.bestViewDirAbs(p.getBodyDir(), viewWidth,
                a, preferred);

        // some corrections....
        double angleToBody = Vektor.normalize(angleAbs - p.getBodyDir());

        // this should never happen
        if (angleToBody < SConf.getInstance().MINNECKANG - neckRotator.getSliceWidth() || angleToBody > SConf.getInstance().MAXNECKANG + neckRotator.getSliceWidth()) {
            System.out.println("NeckRotator breaks neck: C" + getCycle() + " P" + getNumber() + "State:" + a + " result. Neckangle=" + angleToBody + " abs.Angle=" + angleAbs + " BD:" + p.getBodyDir() + " ND:" + p.getNeckAngle());
            neckRotator.showValues(a, 0);
        }

        // as the NeckRotator is discret, it might sometimes return to high
        // values
        // max error is neckRotator.getSliceWidth() (11.5� by default)
        if (angleToBody < SConf.getInstance().MINNECKANG) // + 0.001 for avoiding 180� turn in wrong direction
        {
            angleToBody = SConf.getInstance().MINNECKANG + 0.001;
        }
        if (angleToBody > SConf.getInstance().MAXNECKANG) // - 0.001 for avoiding 180� turn in wrong direction
        {
            angleToBody = SConf.getInstance().MAXNECKANG - 0.001;
        }

        return Vektor.normalize(angleToBody - p.getNeckAngle());
    }

    /**
     * getMyPos returns the current position of the player
     *
     * @return The player's position. Note that if you perform operations on
     *         this Vektor that change it, you need to clone it first!
     */
    public final Vektor getMyPosition() {

        return this.me.getPosition();
    } // end of method getMyPosition()

    /**
     * the age of an dynamic object.
     *
     * @param o -
     *            object
     * @return time in senseBody steps since the object was last seen (or
     *         created).
     *
     */
    public int getAge(DynamicObject o) {

        if (o.getBodyCycle() > this.getBodyCycle()) {
            System.out.println("WM.getAge(DO) called with wrong current Cycle. C-WM:" + this.getBodyCycle() + " C-DO:" + o.getCycle());
        }
        return this.getBodyCycle() - o.getBodyCycle();
    }

    /**
     * returns the cycles since we have seen a given point
     *
     */
    public double getAgeOfPoint(Vektor p) {

        return this.neckRotator.getAgeOfPoint(p);
    }

    /**
     * the cycles since we have seen to a given direction
     *
     * @param dir -
     *            abs dir
     * @return time since the direction was last seen
     */
    public double getAgeOfDir(double dir) {

        return this.neckRotator.getAgeOfDir(dir);
    }

    /**
     * @return true if we play on the left side.
     */
    public boolean isTeamEast() {

        return this.myMaster.isTeamEast();
    }

    /**
     * set wether we play on the left side.
     */
    public void setTeamEast(boolean east) {

        this.myMaster.setTeamEast(east);
    }

    /**
     * @return Ball - the current ball (REF!!)
     */
    public final Ball getBallRef() {

        return this.ball.getBallRef();
    }

    /**
     * @return - The last seen ball (REF!!)
     */
    public final Ball getLastSeenBall() {

        return this.ball.getLastSeenBall();
    }

    /**
     * This method calculates if the player is in an position to kick the ball.
     * Note that due to noise it is possible that this method returns true
     * although the player cannot kick the ball.
     *
     * In fact this method checks if the ball is located in the circle spanned
     * by the radius of the player, the kickable margin and the radius of the
     * ball.
     *
     * @return true if the specified player is in a position to kick the ball.
     */
    public final boolean canKick() {

        return this.getSelfRef().canKick(this.getBallRef());
    }

    /**
     * inOwnPenaltyArea returns true if the DynamicObject with radius size is in
     * own penalty area, false otherwise.
     *
     * @param dynobj - the object
     * @param size -
     *            radius of the DynamicObject
     * @return true if the DynamicObject is in own penalty area, false
     *         otherwise
     */
    public final boolean inOwnPenaltyArea(DynamicObject dynobj, double size) {

        return inOwnPenaltyArea(dynobj.getPosition().x, dynobj.getPosition().y,
                size);
    } // end of method inOwnPenaltyArea()

    /**
     * inOwnPenaltyArea returns true if the DynamicObject with radius size is in
     * own dangerArea, false otherwise.
     *
     * @param dynobj - the object
     * @param size -
     *            radius of the DynamicObject
     * @return true if the DynamicObject is in own penalty area, false
     *         otherwise
     */
    public final boolean inOwnDangerArea(DynamicObject dynobj, double size) {

        return inOwnDangerArea(dynobj.getPosition().x, dynobj.getPosition().y,
                size);
    } // end of method inOwnPenaltyArea()

    /**
     * <code>inOwnPenaltyArea</code> checks if an Object with the coordinates
     * <code>x</code> & <code>y</code> and radius <code>size</code> is in
     * the penalty area, of the player's team. (Also used in
     * getInterceptionPoint)
     *
     * @param x -
     *            X-coordinate
     * @param y -
     *            Y-coordinate
     * @param size -
     *            radius of the object
     * @return boolean - true if the Object is in own penalty area, false
     *         otherwise
     */
    public final boolean inOwnPenaltyArea(double x, double y, double size) {

        if ((x + size < -SConf.getInstance().half_penalty_width) || (SConf.getInstance().half_penalty_width < x - size) || (y + size < -SConf.getInstance().half_length) || (-SConf.getInstance().half_length + SConf.getInstance().penalty_length < y - size)) {
            return false;
        }

        return true;
    }

    /**
     * <code>inOwnDangerArea</code> zisti ci objekt so suradnicami
     * <code>x</code> & <code>y</code> a polomerom <code>size</code>
     * je v nasej nebezpecnej zone pred brankou
     *
     * @param x -
     *            X-coordinate
     * @param y -
     *            Y-coordinate
     * @param size -
     *            polomer objektu
     * @return boolean - true if the Object is in own penalty area, false
     *         otherwise
     */
    public final boolean inOwnDangerArea(double x, double y, double size) {

        if ((x + size < -SConf.getInstance().dangerArea_width) || (SConf.getInstance().dangerArea_width < x - size) || (y + size < -SConf.getInstance().half_length) || (-SConf.getInstance().half_length + SConf.getInstance().dangerArea_length < y - size)) {
            return false;
        }

        return true;
    }

    /**
     * @param b - the ball
     * @return boolean - true if the ball is in own penalty area, else false
     */
    public final boolean inOwnPenaltyArea(Ball b) {

        return inOwnPenaltyArea(b, SConf.getInstance().BALL_SIZE);
    }

    /**
     * @param b - the ball
     * @return boolean - true if the ball is in own penalty area, else false
     */
    public final boolean inOwnDangerArea(Ball b) {

        return inOwnDangerArea(b, SConf.getInstance().BALL_SIZE);
    }

    /**
     * @param player - the player
     * @return boolean - true if the player is in own penalty area, else false
     */
    public final boolean inOwnDangerArea(Player player) {

        return inOwnDangerArea(player, player.getPConf().PLAYER_SIZE);
    }

    /**
     * @param player - the player
     * @return boolean - true if player is in own penalty area, else false
     */
    public boolean inOwnPenaltyArea(Player player) {

        return inOwnPenaltyArea(player, player.getPConf().PLAYER_SIZE);
    }

    /**
     * Checks if the player is in the opponents' penalty area.
     *
     * @return true, if this player is in opponents' penalty area, false
     *         otherwise
     */
    public final boolean inOpponentsPenaltyMe() {
        return inOpponentsPenalty(getSelfRef());
    }

    /**
     * Checks if a player is in opponents' penalty area.
     *
     * @param player
     *            the player to check
     * @return true, if the player is opponents' penalty area, false otherwise
     */
    public boolean inOpponentsPenalty(Player player) {
        return inOpponentsPenalty(player, player.getPConf().PLAYER_SIZE);
    }

    /**
     * Checks if a ball is in opponents' penalty area.
     *
     * @param ball
     *            the ball to check
     * @return true, if the ball is opponents' penalty area, false otherwise
     */
    public boolean inOpponentsPenalty(Ball ball) {
        return inOpponentsPenalty(ball, SConf.getInstance().BALL_SIZE);
    }

    /**
     * Checks if an object is in opponents' penalty area.
     *
     * @param dynobj
     *            the object to check
     * @param size
     *            the size of the object
     * @return true, if the object is in opponents' penalty area
     */
    public boolean inOpponentsPenalty(DynamicObject dynobj, double size) {
        return inOpponentsPenalty(dynobj.getPosition().x,
                dynobj.getPosition().y, size);
    }

    /**
     * Checks if an object is in opponents' penalty area.
     *
     * @param dynobj
     *            the object to check
     * @param size
     *            the size of the object
     * @return true, if the object is in opponents' penalty area
     */
    public boolean inOpponentsPenalty(Vektor point) {
        return inOpponentsPenalty(point.x, point.y, 0);
    }

    /**
     * Checks whether an object is in opponents penalty area.
     *
     * @param x
     *            the X-coordinate of the object
     * @param y
     *            the Y-coordinate of the object
     * @param size
     *            the size of the object
     * @return true, if the object is in opponents' penalty area, false
     *         otherwise
     */
    public boolean inOpponentsPenalty(double x, double y, double size) {
        if ((y + size) < (SConf.getInstance().half_length - SConf.getInstance().penalty_length) || (Math.abs(x) + size > SConf.getInstance().half_penalty_width) || (y + size > SConf.getInstance().half_length)) {
            return false;
        }

        return true;
    }

    /**
     * This method returns a <code>LinkedList</code> of all known teammates
     * that are at a the given <code>angle</code> plus/minus
     * <code>deviation</code> and within the given <code>distance</code>.
     * Note that if you give a <code>deviation</code> of 180, you'll get all
     * teammates within a circle around the player with the radius <code>
     * maxDistance</code>
     *
     * @param angle
     * @param position
     * @param deviation
     * @param maxDistance
     * @return LinkedList of <code>Player</code>s
     */
    public LinkedList<Player> getTeammates(Vektor position, double angle,
            double deviation, double maxDistance) {

        return this.players.getTeammates(position, angle, deviation,
                maxDistance);
    }

    /**
     * Calculates if the position denoted by the Vektor <code>pos</code> is
     * inside the field.
     *
     * @param pos -
     *            Vektor
     * @return true if pos is inside the field.
     */
    public final boolean inField(Vektor pos) {

        if (pos.x > SConf.getInstance().half_width || pos.x < -SConf.getInstance().half_width || pos.y < -SConf.getInstance().half_length || pos.y > SConf.getInstance().half_length) {
            return false;
        }
        return true;
    }

    /**
     * sets a given position back into the field.
     * this is used for the ball as due to noise we might see it outside
     *
     * @param pos -
     *            the position (this position gets changed !!!)
     */
    public final void setIntoField(Vektor pos) {
        if (pos.x > SConf.getInstance().half_width) {
            pos.x = SConf.getInstance().half_width - 0.01;
        }
        if (pos.x < -SConf.getInstance().half_width) {
            pos.x = -SConf.getInstance().half_width + 0.01;
        }
        if (pos.y > SConf.getInstance().half_length) {
            pos.y = SConf.getInstance().half_length - 0.01;
        }
        if (pos.y < -SConf.getInstance().half_length) {
            pos.y = -SConf.getInstance().half_length + 0.01;
        }
    }

    /**
     * @return Player - a new instance of the player
     */
    public final Player getSelfClone() {

        return this.me.getPlayerClone();
    } // end of method getSelf()

    /**
     * isGoalie returns true if the player is a goalkeeper
     *
     * @return boolean - true if player is a goalie
     */
    public final boolean isGoalie() {

        return this.myMaster.isGoalie();
    } // end of method isGoalie()

    /**
     * getBodyAngle returns the body angle of the player body = head - neck
     *
     * @return double - the angle
     */
    public final double getBodyAngle() {

        return this.me.getBodyDir();
    } // end of getBodyAngle()

    /**
     * @param o -
     *            StaticObject
     * @return distance from the player to the StaticObject <code>o</code>.
     */
    public final double getDistance(StaticObject o) {

        return this.getDistance(o.getPosition());
    }

    /**
     * @param v -
     *            Vektor
     * @return distance from the player to the Vektor <code>v</code>.
     */
    public final double getDistance(Vektor v) {

        this.dummyVektor.copy(v);
        this.dummyVektor.subFromThis(this.me.getPosition());
        return this.dummyVektor.getLength();
    }

    /**
     * getOpponents returns an array with all opponents
     *
     * @return Player[] - opponents array (REF!!)
     */
    public final Player[] getOpponents() {

        return this.players.getOpponents();
    }

    /**
     * getTeammates returns an array with all teammates
     *
     * @return Vector - teammates (REF!!)
     */
    public final Player[] getTeammates() {

        return this.players.getTeammates();
    }

    /**
     * getAllPLayers an array with all players
     *
     * @return Vector - all seen players (REF!!)
     */
    public final Player[] getAllPlayers() {

        return this.players.getAllPlayers();
    }

    /**
     * getExpectedPlayers returns an array with all expected players.
     *
     * @return Returns the expectedPlayers (REF!!)
     */
    public final Player[] getExpectedPlayers() {

        return this.players.getExpectedPlayers();
    }

    /**
     * getNextPLayers returns all players as they are expected in the next cycle
     *
     * @return Vector - all next players (REF!!)
     */
    public final Player[] getNextPlayers() {

        return this.players.getNextPlayers();
    }

    /**
     * the opponents goalie if known and null if not known.
     *
     * @return goalie or null if unknown
     */
    public final Player getGoalieOther() {

        for (Player p : this.getOpponents()) {
            if (p.isGoalie()) {
                return p;
            }
        }
        return null;
    }

    /**
     * getMyNextPosition returns the position of the player in the next cycle,
     * assuming, that the player does not dash with current speed.
     *
     * @return Vektor - absolute position
     */
    public final Vektor getMyNextPosition() {

        Vektor v = this.shortTermMemory.get(VEKTORS.MY_NEXT_POSITION);
        if (v == ShortTermMemory.VEKTOR_UNKNOWN) {
            v = getNextPosition(this.getSelfRef());
            this.shortTermMemory.set(VEKTORS.MY_NEXT_POSITION, v);
        }
        return v;
    }

    /**
     * getNextPosition(DynamicObject) returns the position of the given object
     * in the next cycle, assuming, that it's moving with the current speed.
     *
     * @param obj
     * @return Vektor - absolute position
     */
    public final Vektor getNextPosition(StaticObject obj) {

        Vektor nextPos = new Vektor();
        nextPos.copy(obj.getPosition());

        if (obj instanceof DynamicObject) {
            nextPos.addToThis(((DynamicObject) obj).getSpeed());
        }

        return nextPos;
    }

    /**
     * The Players number.
     *
     * @return number of player
     */
    public final int getNumber() {

        if (this.me == null) {
            return -1;
        }
        return getSelfRef().getNumber();
    }

    /**
     * getMySpeed returns the current Speed of the player
     *
     * @return Vektor - speed
     */
    public final Vektor getMySpeed() {

        return this.me.getSpeed();
    }

    /**
     * getViewAngle returns the view angle of the player
     *
     * @return double - width
     */
    public final double getViewWidth() {

        return this.me.getViewWidth();
    } // end of method getViewAngle()

    /**
     * getNeckAngle returns the neck angle of the player neck = head - body
     *
     * @return neck angle
     */
    public final double getNeckAngle() {

        return this.me.getNeckAngle();
    } // end of method getNeckAngle()

    /**
     * getHeadAngle returns the head angle of the player. head = body + neck
     *
     * @return head angle
     */
    public final double getHeadAngle() {

        return this.me.getHeadDir();
    } // end of method getHeadAngle()

    /**
     * Calculates if the specified Player <code>p</code> stays at an offside
     * position.
     *
     * @param p -
     *            Player
     * @param nextCycle -
     *            if true, position of ball in next cycle is used
     * @return true if the player stays at an offside position, else false.
     */
    public final boolean offside(Player p, boolean nextCycle) {

        if (nextCycle) {
            return offside(this.getNextPosition(p), p.isFriend(), nextCycle);
        }
        return offside(p.getPosition(), p.isFriend(), nextCycle);
    }

    /**
     * <code>offside</code> Calculates if the specified Position
     * <code>pos</code> is an offside position.
     *
     * @author Luzian Wild
     * @param pos -
     *            Position
     * @param ourTeam -
     *            calculate offside for friends or opponents.
     * @param nextCycle -
     *            if true, position of ball in next cycle is used
     * @return true if the pos is offside
     *
     */
    public final boolean offside(Vektor pos, boolean ourTeam, boolean nextCycle) {

        double y = getOffsideY(ourTeam);
        double ballY = nextCycle ? this.getNextPosition(this.getBallRef()).y
                : this.getBallRef().getPosition().y;
        if (ourTeam) {
            if (pos.y < ballY) {
                return false;
            }
            if (pos.y > y) {
                return true;
            }
        } else {
            if (pos.y > ballY) {
                return false;
            }
            if (pos.y < y) {
                return true;
            }
        }
        // no offside
        return false;
    }

    /**
     * <code>getOffsideY</code> Calculates the y value of the 'offside' line
     * for the given pos/team.
     *
     * @param pos -
     *            Position
     * @param ourTeam -
     *            calculate offside for friends or opponents.
     * @return the y value of the other team's second last player's position. or
     *         pos.y if no second last player known.
     */
    public final double getOffsideY(boolean ourTeam) {

        if (ourTeam) {
            return this.players.getTeamOffsideY();
        } else {
            return this.players.getOpponentOffsideY();
        }
    }

    /**
     * calculates the velocity of an dynamic object based on the information
     * given in the DynamicObjectInfo. This is taken from the UvA-Trilearn
     * Master thesis.
     *
     * @param doi - the DynamicObjectInfo
     * @param velocity - this vektor will contain the resulting velocity
     *                    avoiding the creation of a new instance
     *                    this vektor is set to "empty" if the info contained
     *                    no speed data (object was to far away)
     *
     *
     */
    public void calculateVelocity(DynamicObjectInfo doi, Vektor velocity) {

        // no visual info about speed
        if (!doi.velocitySeen()) {
            velocity.setEmpty();
            return;
        }

        // see MT P.96 Method 1
        // TODO MT P.97 Method 3; less new value creation
        double r = doi.getDistance();
        double phi_rad = Math.toRadians(doi.getDirection());
        double d_r = doi.getDistChange();
        double d_phi_rad = Math.toRadians(doi.getDirChange());

        double e_rx = Math.cos(phi_rad);
        double e_ry = Math.sin(phi_rad);
        double v_rx = d_r * e_rx - d_phi_rad * r * e_ry;
        double v_ry = d_r * e_ry + d_phi_rad * r * e_rx;

        // swap x <-> y as we use different coordinates
        velocity.pointAt(v_ry, v_rx);
        // turn to get speed in abs. system
        velocity.rotate(this.me.getHeadDir());
        // add own speed
        velocity.addToThis(this.me.getSpeed());
    }

    /**
     * get the closest player to a point. (static version)
     *
     * @param players - the player array to choose from
     * @param point - the point
     * @param allowMe - this player is allowd
     * @return the player closest to a point
     */
    public final Player getClosestPlayerTo(Player[] players, Vektor point, boolean allowMe) {
        return this.players.getClosestPlayerTo(players, point, allowMe);
    }

    /**
     * <code>getClosestOpponentTo</code>
     *
     * @param obj
     * @return Player - opponent with shortest distance to the specified
     *         StaticObject <code>obj</code> or null if there are no opponents
     *         in the player's current world model
     * @see robocup.component.SoccerProphet#getClosestOpponentTo(robocup.component.worldobjects.StaticObject)
     */
    public final Player getClosestOpponentTo(StaticObject obj) {

        return this.players.getClosestOpponentTo(obj.getPosition());
    }

    /**
     * getClosestOpponentTo
     *
     * @param v
     *
     * @return Player - opponent with shortest distance to the Vektor
     *         <code>v</code> or null if there are no opponents in the
     *         player's current world model
     */
    public final Player getClosestOpponentTo(Vektor v) {

        return this.players.getClosestOpponentTo(v);
    }

    /**
     * getStamina()
     *
     * @return double - The player's current stamina.
     */
    public double getStamina() {

        return getSelfRef().getStamina();
    }

    /**
     * getEffort()
     *
     * @return double - The player's current effort.
     */
    public double getEffort() {

        return getSelfRef().getEffort();
    }

    /**
     * Returns the current PlayMode/RefereeMessage.
     *
     * @see robocup.component.ControllerNumbers
     * @return int - gameMode
     */
    public PLAY_MODE getPlayMode() {

        return this.playModeModel.getCurrentPlayMode();
    }

    /**
     * the play mode that was in place before the current playmode.
     *
     * @return
     */
    public PLAY_MODE getPreviousPlaymode() {
        return this.playModeModel.getPreviousPlayMode();
    }

    /**
     * the playmode of the last cycle
     *
     * @return
     */
    public PLAY_MODE getLastCyclesPlaymode() {
        return this.playModeModel.getLastCyclesPlayMode();
    }

    /**
     * the cycle when we got the last play mode info
     *
     * @return
     */
    public int getLastPlayModeMessageCycle() {
        return this.playModeModel.getLastPlayModeMessageCycle();
    }

    /**
     * predicts the effect of an action on an object.
     *
     * @param o -
     *            StaticObject
     * @param a -
     *            Action
     * @return object after action
     */
    public final StaticObject predictNext(StaticObject o, Action a) {

        // dynamic
        if (o instanceof DynamicObject) {
            // ball
            if (o instanceof Ball) {
                Ball b = (Ball) o;
                return b.predictBallAfterAction(this.getSelfRef(), a);
            }
            // player
            if (o instanceof Player) {
                Player p = (Player) o;
                return p.predictPlayerAfterAction(a);
            }
            System.out.println("Unknown moving object in Prophet.predictNext()");
        }
        // static
        return o;
    }

    /**
     * Tells if an object should be seen.
     *
     * @param o
     * @return true if object in viewCone or visibleDistance
     */
    public boolean shouldBeSeen(StaticObject o) {

        return this.me.shouldBeSeen(o);
    }

    /**
     * @return Returns the viFlag.
     */
    public final boolean isViBased() {

        return this.viBased;
    }

    /**
     * @return returns the body cycle
     */
    public final int getBodyCycle() {

        return this.bodyCycleCounter;
    }

    /**
     *
     * @return the players player config.
     */
    public PConf getMyPConf() {

        return getSelfRef().getPConf();
    }

    /**
     * @return a reference to myself. DON'T MESS AROUND WITH IT.
     */
    public Player getSelfRef() {

        return this.self;
    }

    /**
     * @return true if player moved this cycle
     */
    public boolean hasMoved() {

        return this.getCycle() == this.lastMoveCycle;
    }

    /**
     * @return true if the ball is in the kickable range of another player.
     */
    public boolean ballCanBeKickedByOther() {

        for (Player p : this.getPlayersThatCanKick()) {
            if (!p.isMe()) {
                return true;
            }
        }
        return false;
    }

    /**
     * @return true if the ball is in the kickable range of an opponent.
     */
    public boolean ballCanBeKickedByOpponent() {

        for (Player p : this.getPlayersThatCanKick()) {
            if (!p.isFriend()) {
                return true;
            }
        }
        return false;
    }

    /**
     * @return a list of players that have the ball in their kickable range.
     */
    public LinkedList<Player> getPlayersThatCanKick() {

        LinkedList<Player> result = this.getPlayersThatCanKick(this.getBallRef(), this.getAllPlayers());
        return result;
    }

    /**
     * filters all players that can't kick the ball because they are to far.
     *
     * @param b
     * @param player
     * @return
     */
    public LinkedList<Player> getPlayersThatCanKick(Ball b, Player[] player) {

        return this.players.getPlayersThatCanKick(b, player);
    }

    /**
     * This method checks if the next playmode is kickoff own (only available in
     * playmode before kickoff)
     *
     * @return true, if the next playmode is kick-off own
     */
    public boolean isKickOffOwn() {

        // needed values
        int halfTime = (int) (SConf.getInstance().HALF_TIME * 10);
        int cycle = this.getCycle();
        boolean teamEast = this.myMaster.isTeamEast();

        // is kick-off?
        if (cycle % halfTime == 0) {
            // teamEast && 2.KickOff -> true
            if (teamEast) {
                if (cycle % (2 * halfTime) != 0) {
                    return true;
                } else {
                    return false;
                }
            } else {
                // team West && 1.KickOff ->
                if (cycle % (2 * halfTime) == 0) {
                    return true;
                } else {
                    return false;
                }
            }
        }
        //TODO check who scored last
        return false;
    }

    /**
     * a string containing the current cycle and the players number
     *
     * @return
     */
    public String getCycleAndPlayerString() {

        return this.getCycle() + ". P:" + this.getNumber();
    }

    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // Mental-Model Methods
    public Vektor getSelfPositionEstimationError() {

        return this.me.getSelfPositionEstimationError();
    }

    /**
     * the opponents goalie.
     *
     * @return the opponents goalie; null if unknown
     */
    public Player getOpponentsGoalie() {

        return this.players.getOpponentsGoalie();
    }

    /**
     * This method retrieves the name of the own team
     */
    public String getTeamName() {

        return this.myMaster.getTeamName();
    }

    /**
     * @return all player configs
     */
    public PConf[] getAvailablePconfs() {

        return this.configs;
    }

    /**
     * @return the shortTermMemory
     */
    public ShortTermMemory getShortTermMemory() {

        return this.shortTermMemory;
    }

    /**
     * @return the longTermMemory.
     */
    public LongTermMemory getLongTermMemory() {

        return this.longTermMemory;
    }

    /**
     * The current server cycle.
     *
     * @return
     */
    public final int getCycle() {

        if (getSelfRef() == null) {
            return -1;
        }
        return getSelfRef().getCycle();
    }

    /**
     * This method retrieves a player from our team or of the opponents team by
     * the given number. If <team> is true, our team is considered, else the
     * opponents team. The numbers must be from 1..11
     *
     * @param num -
     *            the players number
     * @param team -
     *            our team?
     * @return the player
     */
    public Player getPlayerByNumber(int num, boolean team) {

        Player[] players = team ? getTeammates() : getOpponents();
        for (Player p : players) {
            if (p.getNumber() == num) {
                return p;
            }
        }
        return null;
    }

    /**
     * Retrieves the message-factory
     */
    public MessageFactory getMessageFactory() {

        return this.mf;
    }

    /**
     * @return the self as it was predicted
     */
    public Player getExpectedSelf() {

        return this.me.getPredictedMe();
    }

    /**
     * @return
     */
    public Player getLastMe() {

        return this.me.getLastMe();
    }

    /**
     * @return the player number of the last heared player
     */
    public int getLastHearedMessageSender() {

        return this.longTermMemory.getHearedMessageSender(0);
    }

    /**
     * @return the last heared message type
     */
    public MESSAGES getLastHeardMessageType() {

        return this.longTermMemory.getHearedMessageType(0);
    }

    /**
     * @return goals(we) - goals(other)
     */
    public int getGoalDiff() {
        return this.goalsTeam - this.goalsOther;
    }

    /**
     * @param play_on - the play mode to be set
     */
    public void correctPlayMode(PLAY_MODE play_on) {

        this.playModeModel.correctPlayMode(play_on);
    }

    /**
     *
     * @return true if we collided with something
     */
    public boolean collided() {
        return this.collisionModel.collided();
    }

    /**
     * returns predefined sorted player arrays.
     *
     * @param sorted -
     *            the sort mode and type
     * @param position -
     *            the position needed for sorting (i.e. by distance or angle)
     * @return an array containing the sorted player references
     */
    public Player[] getPlayersSortedBy(PlayersModel.SORTED sorted,
            Vektor position) {
        return this.players.getPlayersSortedBy(sorted, position);
    }

    /**
     * gets the side were most opponents are (left, right, none).
     *
     * @return
     */
    public DOMINATED_SIDE getDominatedSide() {
        return this.players.getDominatedSide();
    }

    /**
     * @return Returns the opponentTeamName.
     */
    public String getOpponentTeamName() {

        return this.opponentTeamName;
    }

    /**
     * @param opponentTeamName
     *            The opponentTeamName to set.
     */
    public void setOpponentTeamName(String opponentTeamName) {

        this.opponentTeamName = opponentTeamName;
    }

    /**
     * @return the {@link CLangModel}
     */
    public CLangModel getClangModel() {
        return clangModel;
    }

    /**
     * sets the clang model.
     *
     * @param clangModel - {@link CLangModel}
     */
    public void setClangModel(CLangModel clangModel) {

        this.clangModel = clangModel;
    }

    /**
     * @return goals scored by opponent
     */
    public int getGoalsOther() {

        return goalsOther;
    }

    /**
     * @return goals scored
     */
    public int getGoalsTeam() {

        return goalsTeam;
    }

    /**
     * resets the particle filter after a collision.
     */
    public void resetFilterAfterCollisionWithBall() {
        this.ball.resetFilterAfterCollisionWithBall();
        this.me.resetFilterAfterCollisionWithBall();
    }
} // end of class WorldModel
